﻿/********************************************************************
	created:	2013/05/02
	created:	2:5:2013   14:33
	filename: 	d:\Lidar\Source\src\GlobalSurface\Spline.h
	file path:	d:\Lidar\Source\src\GlobalSurface
	file base:	Spline
	file ext:	h
	author:		胡翰
	
	purpose:	拟合一张Thin Plate Spline
				Spline z(x,y) = a0 + a1x + a2y + SIGMA_0_p(wi * R(ri))
				ri = ||(xi,yi)-(x,y)||
				R(r) = r^2 * log(r)
				拟合方法 - http://elonen.iki.fi/code/tpsdemo/
*********************************************************************/
#pragma once
#include <pcl/pcl_base.h>
#include <pcl/point_types.h>

#include <Eigen/Core>

using namespace Eigen;
using namespace pcl;

namespace oil
{
class Spline : public PCLBase<PointXYZ>
{
public:
	typedef PCLBase<PointXYZ>::PointCloud PointCloud;
	typedef PCLBase<PointXYZ>::PointCloudPtr PointCloudPtr;

	using PCLBase<PointXYZ>::indices_;
	using PCLBase<PointXYZ>::input_;

	/*************************************************************************/
	/**
	* @Synopsis		Spline
	* @Param		const PointCloudPtr & input			输入控制点
	* @Param		const Vector2f & min				最小范围
	* @Param		const Vector2f & max				最大范围
	* @Param		double lambda						正则化权值
	* @Returns
	* @remark
	*/
	/*************************************************************************/
	Spline(const PointCloudPtr& input, double lambda);
	Spline() {};
	~Spline();

	/*************************************************************************/
	/**
	* @Synopsis		getLambda							获取平滑权值
	* @Returns		double
	* @remark
	*/
	/*************************************************************************/
	double getLambda() const { return lambda_; }

	/*************************************************************************/
	/**
	* @Synopsis		setLambda							设置平滑权值
	* @Param		double val
	* @Returns		void
	* @remark
	*/
	/*************************************************************************/
	void setLambda(double val) { lambda_ = val; }

	/*************************************************************************/
	/**
	* @Synopsis		getBending
	* @Returns		double
	* @remark
	*/
	/*************************************************************************/
	double getBending() const { return bending_; }

	/*************************************************************************/
	/**
	* @Synopsis		getStDeviation
	* @Returns		double
	* @remark
	*/
	/*************************************************************************/
	double getStDeviation() const { return std_; }

	/*************************************************************************/
	/**
	* @Synopsis		interpolate							内插一个点云
	* @Param		PointCloudPtr & cloud
	* @Returns		void
	* @remark
	*/
	/*************************************************************************/
	void interpolate(PointCloudPtr& cloud);

	/*************************************************************************/
	/**
	* @Synopsis		interpolate							内插一个点
	* @Param		PointXYZ & point
	* @Returns		void
	* @remark
	*/
	/*************************************************************************/
	void interpolate(PointXYZ& point);

	/*************************************************************************/
	/**
	* @Synopsis		interpolate							内插点云
	* @Param		PointCloud & points
	* @Returns		void
	* @remark
	*/
	/*************************************************************************/
	void interpolate(PointCloud& points);

	/*************************************************************************/
	/**
	* @Synopsis		calcTPS							计算Thin plate splie的参数
	* @Returns		void
	* @remark
	*/
	/*************************************************************************/
	void calcTPS();

private:
	/// 平滑权值
	double lambda_;

	/// spline的参数方程
	VectorXd v_; //[w0 w1 ... wp-1 a0 a1 a2]

	/// bending energy, 弯曲度
	double bending_;

	///data cost
	double cost_;

	///标准差
	double std_;

private:
	/*************************************************************************/
	/**
	* @Synopsis		radialFunction						radial函数
	* @Param		double r
	* @Returns		double
	* @remark		计算radial 函数 R(r) = r^2 * log(r)
	*/
	/*************************************************************************/
	inline double radialFunction(double r);

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

};


}//end oil
